//! 创新点：非常关键的函数，构建使用Wasserstein距离的GICP约束（使用Cauchy）
//前4次迭代：将点云中的点mean_A转换到世界坐标下，得到trans_mean_A，并根据delta_P_，在地图中寻找mean_B，计算mean_A和mean_B生成的残差项。2次迭代之后进行卡方检验，剔除外点
//4次以后迭代：根据前三次生成的对应关系，更新trans_mean_A，与对应的mean_B计算残差项
double LIO::ConstructWassersteinGICPConstraints(Eigen::Matrix<double, 15, 15>& H, Eigen::Matrix<double, 15, 1>& b) 
{
  Eigen::Matrix<double, 8, 6> result_matrix = Eigen::Matrix<double, 8, 6>::Zero();
  Eigen::Matrix<double, 8, 6> init_matrix = Eigen::Matrix<double, 8, 6>::Zero();
  //4次迭代以后
  if (need_converge_) 
  {
    result_matrix = tbb::parallel_reduce(tbb::blocked_range<size_t>(0, Max_Group_second.size()),init_matrix,[&, this](tbb::blocked_range<size_t> r,Eigen::Matrix<double, 8, 6> local_result) 
    {
      for (size_t i = r.begin(); i < r.end(); ++i) 
      {
        //将mean_A转换到世界坐标系下，得到trans_mean_A
        Eigen::Vector3d trans_mean_A = curr_state_.pose.block<3, 3>(0, 0) * Max_Group_second[i]->mean_A +curr_state_.pose.block<3, 1>(0, 3);
        //计算trans_mean_A和地图中mean_B之间的误差
        double exp_temp = 1.0;
#ifdef YEL
        //! 创新点：改为Wasserstein距离 W(p,q)=||Uq-Up||^2+Tr(Cp)+Tr(Cq)-2Tr[(CqCp)^0.5]
        exp_temp = exp(0.1*fabs(Max_Group_second[i]->tr_A+Max_Group_second[i]->tr_B-2.0*Max_Group_second[i]->tr_AB));
#endif
        Eigen::Vector3d error = exp_temp*(Max_Group_second[i]->mean_B - trans_mean_A);
        // 鲁棒损失函数计算
        Eigen::Matrix3d mahalanobis = Max_Group_second[i]->mahalanobis;//由前三次迭代得到

        double cost_function = error.transpose() * mahalanobis * error;
        //根据cost_function的大小计算rho
        Eigen::Vector3d rho;
        CauchyLossFunction(cost_function, 10.0, rho);

        local_result(7, 0) += config_.gicp_constraint_gain * rho[0];

        // 误差相对于状态量的雅可比矩阵
        Eigen::Matrix<double, 3, 6> dres_dx = Eigen::Matrix<double, 3, 6>::Zero();

        // 误差相对于旋转的雅可比矩阵
        dres_dx.block<3, 3>(0, 0) = exp_temp*curr_state_.pose.block<3, 3>(0, 0) * Sophus::SO3d::hat(Max_Group_second[i]->mean_A);

        // 误差相对于位置的雅可比矩阵
        dres_dx.block<3, 3>(0, 3) = -exp_temp*Eigen::Matrix3d::Identity();

        Eigen::Matrix3d robust_information_matrix = config_.gicp_constraint_gain * (rho[1] * mahalanobis + 2.0 * rho[2] * mahalanobis * error * error.transpose() * mahalanobis);
        
        local_result.block(0, 0, 6, 6) += dres_dx.transpose() * robust_information_matrix * dres_dx;
        local_result.block(6, 0, 1, 6) += (config_.gicp_constraint_gain * rho[1] * dres_dx.transpose() * mahalanobis * error).transpose();
      }

      return local_result;
    },[](Eigen::Matrix<double, 8, 6> x, Eigen::Matrix<double, 8, 6> y) {return x + y;});

      H.block<6, 6>(IndexErrorOri, IndexErrorOri) += result_matrix.block<6, 6>(0, 0);
      b.block<6, 1>(IndexErrorOri, 0) +=result_matrix.block<1, 6>(6, 0).transpose();

      return result_matrix(7, 0);
  }

  //if(iter_num_==3)
  {
    //! 创新：：重新建立当前帧点云的voxel
    fast_voxel_grid_ptr_->Filter(filtered_cloud_ptr, cloud_DS_ptr_, cloud_cov_ptr_,curr_state_.pose.block<3,3>(0,0), curr_state_.pose.block<3,1>(0,3));
  }
  //前4次迭代
  size_t delta_p_size = voxel_map_ptr_->delta_P_.size();
  //获取当前点云中点的数量
  size_t N = cloud_cov_ptr_->size();
  //清空存储点云与地图匹配结果的数组
  correspondences_array_.clear();

  size_t new_hash_valid = 0;
  size_t old_hash_valid = 0;
  size_t hash_invalid = 0;
  size_t invalid_point = 0;
  size_t valid_point = 0;
  size_t not_zero_voxel=0;
  size_t dis_invalid_point = 0;

  size_t match1 = 0;size_t match2 = 0;size_t match3 = 0;size_t match4 = 0;size_t match5 = 0;size_t match6 = 0;size_t match7 = 0;
  size_t invalid_point_chi2_error=0;

  size_t cannotfind_match;

  result_matrix = tbb::parallel_reduce(tbb::blocked_range<size_t>(0, N),init_matrix,[&, this](tbb::blocked_range<size_t> r,Eigen::Matrix<double, 8, 6> local_result) 
  {
    //! 将点云中的点mean_A转换到世界坐标下，得到trans_mean_A，并根据delta_P_，在地图中寻找mean_B，计算mean_A和mean_B生成的残差项。2次迭代之后进行卡方检验，剔除外点
    for (size_t i = r.begin(); i < r.end(); ++i) 
    {
      //! 注意：这里的points[i]指的是对应的体素质心点。针对点云中的每个体素质心点，提取其协方差并转换到当前状态的世界坐标系中
      const PointCovType& point_cov = cloud_cov_ptr_->points[i];
      //getVector3fMap函数是PCL（Point Cloud Library）库中的一个成员函数，用于获取PointT类型对象的Eigen向量表示
      const Eigen::Vector3d mean_A = point_cov.getVector3fMap().cast<double>();
      const Eigen::Vector3d trans_mean_A = curr_state_.pose.block<3, 3>(0, 0) * mean_A + curr_state_.pose.block<3, 1>(0, 3);

      Eigen::Matrix3d cov_A;
      cov_A << point_cov.cov[0], point_cov.cov[1], point_cov.cov[2],
                point_cov.cov[1], point_cov.cov[3], point_cov.cov[4],
                point_cov.cov[2], point_cov.cov[4], point_cov.cov[5];

      Eigen::Vector3d mean_B = Eigen::Vector3d::Zero();
      Eigen::Matrix3d cov_B = Eigen::Matrix3d::Zero();

      std::shared_ptr<Correspondence> corr_ptr = std::make_shared<Correspondence>();
      corr_ptr->tr_A = cov_A.trace();

      //A中体素中心和体素质心的距离
      float dis_A_x = std::fmod(trans_mean_A[0], 0.5);
      float dis_A_y = std::fmod(trans_mean_A[1], 0.5);
      float dis_A_z = std::fmod(trans_mean_A[2], 0.5);
      float dis_A = std::sqrt(dis_A_x * dis_A_x + dis_A_y * dis_A_y + dis_A_z * dis_A_z);
      Eigen::Vector3d v_dis_A{dis_A_x,dis_A_y,dis_A_z};
      size_t hash_idx_A = fast_voxel_grid_ptr_->ComputeHashIndex(trans_mean_A);
      
      MyAccessor accessor;
      int point_in_A =0;
      if (fast_voxel_grid_ptr_->voxel_map_ptr_->find(accessor, hash_idx_A)) 
      {
         point_in_A = accessor->second->N_;
        //TODO::输出信息表明，几乎百分之80左右的point_in_A都为1、2，也就是说，他们的协方差应该是0，协方差并不可信啊！！！！！
        if(point_in_A > 2)
        {
          valid_point++;
          //TODO:：仍然没有解决，如果是1的话，协方差应该是0，但输出中有的并不是零
          //LOG (INFO) << "point_in_A: " << point_in_A << "cov_A: " << cov_A;
          //continue;
        }
      }

      //对于每个转换的点，寻找其在体素地图中邻近(7个)的点，并获取其对应的质心和协方差
      bool find = false;
      for (size_t i = 0; i < delta_p_size; ++i) 
      {
        Eigen::Vector3d nearby_point = trans_mean_A + voxel_map_ptr_->delta_P_[i];
        //通过 Hash 索引来加速查找，提高处理效率
        size_t hash_idx_B = voxel_map_ptr_->ComputeHashIndex(nearby_point);

        //如果在同一个格子里，就是找到了！
        if (voxel_map_ptr_->GetCentroidAndCovariance(hash_idx_B, mean_B, cov_B) && voxel_map_ptr_->IsSameGrid(nearby_point, mean_B)) 
        {
          //B中体素中心和体素质心的距离
          float dis_B_x = std::fmod(mean_B[0], 0.5);
          float dis_B_y = std::fmod(mean_B[1], 0.5);
          float dis_B_z = std::fmod(mean_B[2], 0.5);
          float dis_B = std::sqrt(dis_B_x * dis_B_x + dis_B_y * dis_B_y + dis_B_z * dis_B_z);
          Eigen::Vector3d v_dis_B{dis_B_x,dis_B_y,dis_B_z};
          //! 创新点：如果距离小于等于半径，则认为找到了
          if (fabs(dis_B - dis_A)> 0.25) 
          {
            dis_invalid_point++;
            //continue;
          }

          //计算马氏距离以评估当前点云点与找到的对应点之间的误差
          Eigen::Matrix3d mahalanobis = (cov_B + curr_state_.pose.block<3, 3>(0, 0) * cov_A *curr_state_.pose.block<3, 3>(0, 0).transpose() + Eigen::Matrix3d::Identity() * 1e-3).inverse();
          double exp_temp = 1.0;
#ifdef YEL
          //! 创新点：计算关于协方差矩阵迹的数据
          corr_ptr->tr_B = cov_B.trace();
          Eigen::Matrix3d cov_AB = cov_A*cov_B;
          corr_ptr->tr_AB =MatrixSqrt(cov_AB, cov_AB.rows()).trace();
          exp_temp =exp(0.1*abs(corr_ptr->tr_A+corr_ptr->tr_B-2.0*corr_ptr->tr_AB));
#endif
          //如果当前体素中的点很少，则不适用cov，没有意义
          if(point_in_A<6)
          {
            //exp_temp = 1.0;
            //corr_ptr->tr_A = 0.0;
            //corr_ptr->tr_B = 0.0;
            //corr_ptr->tr_AB = 0.0; 
          }
          Eigen::Vector3d error = (mean_B - trans_mean_A)*exp_temp;
          //Eigen::Vector3d temp_error = (v_dis_B - v_dis_A)*exp_temp;

          //通过卡方检验剔除外点，保证匹配点的可靠性
          double chi2_error = error.transpose() * mahalanobis * error;
          //double temp_chi2_error = temp_error.transpose() * mahalanobis * temp_error;
          //LOG(INFO) << i<<"中心距离差: "<<fabs(dis_B - dis_A)<<" chi2_error: " << chi2_error << " temp_chi2_error: " << temp_chi2_error;
          //LOG(INFO) << "v_dis_B: " << v_dis_B.transpose() << " v_dis_A: " << v_dis_A.transpose() << " error: " << error.transpose() << " mahalanobis: " << mahalanobis << " exp_temp: " << exp_temp;

          //2次迭代之后，即第三次时，进行卡方检验，剔除外点
          if (config_.enable_outlier_rejection) 
          {
            if (iter_num_ > 2 && chi2_error > 7.815) 
            {
              invalid_point_chi2_error++;
              continue;
            }
          }

          switch (i) 
          {
            case 0:
              match1++;
              break;
            case 1:
              match2++;
              break;
            case 2:
              match3++;
              break;
            case 3:
              match4++;
              break;
            case 4:
              match5++;
              break;
            case 5:
              match6++;
              break;
            case 6:
              match7++;
              break;
          }

          //存储当前点云点与找到的对应点之间的对应关系
          corr_ptr->mean_A = mean_A;
          corr_ptr->mean_B = mean_B;
          corr_ptr->mahalanobis = mahalanobis;
          //存储对应关系
          correspondences_array_.emplace_back(corr_ptr);

          // loss function
          double cost_function = chi2_error;

          Eigen::Vector3d rho;
          //接着调用CauchyLossFunction，这是一个鲁棒损失函数，用于减小异常值对优化结果的影响。该函数返回的rho向量通常包含了代价函数的各类信息（如残差和对应的梯度）
          CauchyLossFunction(cost_function, 10.0, rho);
          //根据GICP约束增益config_.gicp_constraint_gain和rho[0]的值，更新局部结果
          local_result(7, 0) += config_.gicp_constraint_gain * rho[0];

          //存储残差对于状态变量的偏导数
          Eigen::Matrix<double, 3, 6> dres_dx = Eigen::Matrix<double, 3, 6>::Zero();

          //通过当前状态的姿态curr_state_.pose和mean_A（相对于当前点云的值），计算旋转的偏导数
          dres_dx.block<3, 3>(0, 0) = exp_temp*curr_state_.pose.block<3, 3>(0, 0) * Sophus::SO3d::hat(mean_A);

          //将偏导数的最后3列设置为负单位矩阵，表示位置对误差的影响
          dres_dx.block<3, 3>(0, 3) = -exp_temp*Eigen::Matrix3d::Identity();

          //创建了一个鲁棒信息矩阵，使用了mahalanobis（马氏距离的协方差）以及之前计算的rho，将这些结合以确保优化具有鲁棒性
          Eigen::Matrix3d robust_information_matrix = config_.gicp_constraint_gain * (rho[1] * mahalanobis + 2.0 * rho[2] * mahalanobis * error * error.transpose() * mahalanobis);

          //使用计算得到的偏导数和鲁棒信息矩阵更新local_result，将偏导数的转置与鲁棒信息矩阵乘积的结果加到局部结果
          local_result.block(0, 0, 6, 6) += dres_dx.transpose() * robust_information_matrix * dres_dx;

          //将通过加权计算的偏导数、马氏距离和误差（error）的乘积更新局部结果的另一部分
          local_result.block(6, 0, 1, 6) += (config_.gicp_constraint_gain * rho[1] * dres_dx.transpose() * mahalanobis * error). transpose();
          //LOG(INFO) <<"-----找到对应点-----:"<<mean_A[0]<< "   "<<mean_A[1]<<"   "<<mean_A[2];
          //! TODO：：这个break过于草率，怎么知道哪个是最合适的体素呢！应该有一个评判的标准，比如：距离、质心、协方差、卡方值等。
          find = true;
          break;
        }
      }
      //if(!find)
      //  LOG(INFO) <<"没有找到对应点！"<<mean_A[0]<< "   "<<mean_A[1]<<"   "<<mean_A[2];
    }//每个点只要找到一个对应，就不再对7个临近的grid进行搜索
    return local_result;
    },[](Eigen::Matrix<double, 8, 6> x, Eigen::Matrix<double, 8, 6> y) 
    {
      return x + y;
    });

    LOG (INFO) <<"匹配点数量："<< correspondences_array_.size()<<"  valid_point: " << valid_point <<"  invalid_point: " << invalid_point << " dis_invalid_point: " << dis_invalid_point;
    LOG (INFO) <<"match1: "<< match1 << " match2: "<< match2 << " match3: "<< match3 << " match4: "<< match4 << " match5: "<< match5 << " match6: "<< match6 << " match7: "<< match7;
    LOG (INFO) << "没有找到点："<< N-(match1+match2+match3+match4+match5+match6+match7)<<"  invalid_point_chi2_error:"<<invalid_point_chi2_error;
    //LOG(INFO) << "new_hash_valid: " << new_hash_valid << " old_hash_valid: " << old_hash_valid<< " 重复无效hash_invalid: " << hash_invalid;

    //TODO::通过上述直接投影方法，没有找到点还是特别多的啊！对这些没有找到的点，也可以进行投影找点啊对不对！

/************yel add start*********/
    if(iter_num_==3)
    {
      Max_Group_second.clear();
      if(correspondences_array_.size()<800)
      {
        for(int i=0;i<correspondences_array_.size();i++)
        {
            Max_Group_second.push_back(correspondences_array_[i]);
        }
        LOG(WARNING) << "Max_Group_second size<800: " << Max_Group_second.size();
      }
      else
      {
        Graphs.clear();
        SearchGroups();
        //SearchGroups_old(); //SearchGroups_V2();
      }
    }

    effect_feat_num_ = correspondences_array_.size();
    H.block<6, 6>(IndexErrorOri, IndexErrorOri) += result_matrix.block<6, 6>(0, 0);
    b.block<6, 1>(IndexErrorOri, 0) += result_matrix.block<1, 6>(6, 0).transpose();

    return result_matrix(7, 0);
}